#include "PID.h"

void PID_Init(PID_t *pid, float kp, float ki, float kd, float out_min, float out_max) {
  pid->Kp = kp;
  pid->Ki = ki;
  pid->Kd = kd;

  pid->setpoint = 0.0f;
  pid->measured = 0.0f;

  pid->error = 0.0f;
  pid->last_error = 0.0f;
  pid->integral = 0.0f;
  pid->derivative = 0.0f;

  pid->output = 0.0f;
  pid->output_min = out_min;
  pid->output_max = out_max;
}

float PID_Compute(PID_t *pid, float setpoint, float measured) {
  pid->setpoint = setpoint;
  pid->measured = measured;

  pid->error = setpoint - measured;

  // 积分项
  pid->integral += pid->error;

  // 抗积分饱和处理
  if (pid->integral * pid->Ki > pid->output_max)
    pid->integral = pid->output_max / pid->Ki;
  else if (pid->integral * pid->Ki < pid->output_min)
    pid->integral = pid->output_min / pid->Ki;

  // 微分项
  pid->derivative = pid->error - pid->last_error;

  // PID 计算（位置式）
  pid->output = pid->Kp * pid->error + pid->Ki * pid->integral + pid->Kd * pid->derivative;

  // 限幅
  if (pid->output > pid->output_max)
    pid->output = pid->output_max;
  else if (pid->output < pid->output_min)
    pid->output = pid->output_min;

  // 更新误差
  pid->last_error = pid->error;

  return pid->output;
}
